import cv2
import numpy as np
import matplotlib.pyplot as plt
import glob


# 车道线提取
# 颜色空间转换——》边缘检测——》颜色阈值-》合并并且使用L通道进行白的区域的抑制
def pipeline(img, s_thresh=(170, 255), sx_thresh=(40, 200)):
    # 复制原图像
    img = np.copy(img)
    # 颜色空间转换
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(float)# np.float
    l_chanel = hls[:, :, 1]
    s_chanel = hls[:, :, 2]
    # sobel边缘检测
    sobelx = cv2.Sobel(l_chanel, cv2.CV_64F, 1, 0)
    # 求绝对值
    abs_sobelx = np.absolute(sobelx)
    # 将其装换为8bit的整数
    scaled_sobel = np.uint8(255 * abs_sobelx / np.max(abs_sobelx))
    # 对边缘提取结果进行二值化
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1
    # plt.figure()
    # plt.imshow(sxbinary, cmap='gray')
    # plt.title("sobel")
    # plt.show()

    # s通道阈值处理
    s_binary = np.zeros_like(s_chanel)
    s_binary[(s_chanel >= s_thresh[0]) & (s_chanel <= s_thresh[1])] = 1
    # plt.figure()
    # plt.imshow(s_binary, cmap='gray')
    # plt.title("schanel")
    # plt.show()
    # 结合边缘提取结果和颜色的结果，
    color_binary = np.zeros_like(sxbinary)
    color_binary[((sxbinary == 1) | (s_binary == 1)) & (l_chanel > 100)] = 1
    #cv2.imshow("123",color_binary*255)
    return color_binary

#获取掩盖模
def get_rio(img,weight,height):
    # 定义多边形区域的顶点坐标 
    dx = weight / 1000
    dy = height / 1000
    pts = np.array([[1 + dx*0, height], 
                    [weight - dx*0, height], 
                    [weight - dx*0, height - int(dy*400)],
                    [1 + dx*0, height - int(dy*400)],], np.int32)  
    # pts = np.array([[10, 50], [200, 50], [200, 200], [10, 200]], np.int32)
    #创建一个与原始图像大小相同的全黑色掩膜  
    mask1 = np.zeros(img.shape, img.dtype)  
    cv2.fillConvexPoly(mask1, pts, (255, 255, 255))  
    # 使用掩码将源图像中的ROI区域复制到输出图像  
    outimage = cv2.bitwise_and(img, img, mask=mask1)  
    # cv2.imshow("123",outimage*255)
    return outimage
# 精确定位车道线
def cal_line_param(binary_warped,src):
    #获取掩模部分图像
    weight = binary_warped.shape[1]
    height = binary_warped.shape[0]
    rio_image = get_rio(binary_warped,weight,height)
    # cv2.imshow("123",rio_image)
    # 1.确定左右车道线的位置
    # 统计直方图
    histogram = np.sum(rio_image[-1:-40:-1, :], axis=0)
    # 在统计结果中找到左右最大的点的位置，作为左右车道检测的开始点
    # 将统计结果一分为二，划分为左右两个部分，分别定位峰值位置，即为两条车道的搜索位置
    midpoint = int(histogram.shape[0] / 2) # np.int(histogram.shape[0] / 2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint
    # 2.滑动窗口检测车道线
    # 设置滑动窗口的数量，计算每一个窗口的高度
    nwindows = 30
    window_height = int(rio_image.shape[0] / nwindows) # np.int(binary_warped.shape[0] / nwindows)
    # 获取图像中不为0的点
    nonzero = rio_image.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # 车道检测的当前位置
    leftx_current = leftx_base
    rightx_current = rightx_base
    # 设置x的检测范围，滑动窗口的宽度的一半，手动指定
    margin = 50
    # 设置最小像素点，阈值用于统计滑动窗口区域内的非零像素个数，小于50的窗口不对x的中心值进行更新
    minpix = 20
    # 用来记录搜索窗口中非零点在nonzeroy和nonzerox中的索引
    left_lane_inds = []
    right_lane_inds = []

    # 遍历该副图像中的每一个窗口
    leftline_x = [leftx_current] #每个窗口的中心点x值
    leftline_y = [binary_warped.shape[0]] #每个窗口的中心点y值
    for window in range(nwindows):
        # 设置窗口的y的检测范围，因为图像是（行列）,shape[0]表示y方向的结果，上面是0
        win_y_low = binary_warped.shape[0] - (window + 1) * window_height
        win_y_high = binary_warped.shape[0] - window * window_height
        # 左车道x的范围
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        # 右车道x的范围
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        # 确定非零点的位置x,y是否在搜索窗口中，将在搜索窗口内的x,y的索引存入left_lane_inds和right_lane_inds中
        #nonzerox,y中存放点的位置信息,索引值为点的id，通过以下筛选，能够筛选出窗口范围内的点的id
        #满足条件的点的x索引数组，在nonzerox中的索引
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
                          (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
                           (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        #print(left_lane_inds)
        # 如果获取的点的个数大于最小个数，则利用其更新滑动窗口在x轴的位置
        if len(good_left_inds) > minpix:
            mean_x = np.mean(nonzerox[good_left_inds])
            leftline_x.append(mean_x)
            leftline_y.append(0.5*(win_y_high+win_y_low))
            leftx_current = int(mean_x) # np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:
            mean_x = np.mean(nonzerox[good_right_inds])
            rightx_current = int(np.mean(mean_x)) # np.int(np.mean(nonzerox[good_right_inds]))
    leftline_point = np.array([leftline_x,leftline_y]).T
    # 将检测出的左右车道点转换为array,数组每个值是对应点在nonzero数组的索引值
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)
    # 获取检测出的左右车道点在图像中的位置
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    #right_fit = np.polyfit(righty, rightx, 2)
    # 3.用曲线拟合检测出的点,二次多项式拟合，返回的结果是系数
    if len(lefty) > 0:
        left_fit = np.polyfit(lefty, leftx, 2)
    else:
        left_fit = [0,0,0]
    print(left_fit)
    #right_fit = np.polyfit(righty, rightx, 2)
    # 获取图像的行数
    y_max = binary_warped.shape[0]
    # 在拟合曲线中获取左右车道线的像素位置
    left_points = [[left_fit[0] * y ** 2 + left_fit[1] * y + left_fit[2], y] for y in range(360,y_max)]
    #right_points = [[right_fit[0] * y ** 2 + right_fit[1] * y + right_fit[2], y] for y in range(y_max - 1, -1, -1)]
    out_img = src
    for point in left_points:
        out_img = cv2.circle(out_img, (int(point[0]),int(point[1])), 2, [0,0,255], -1)  
    #cv2.imshow("out_img",out_img)
    return leftline_point,out_img


# 填充车道线
def fill_lane_poly(img, line_point):
    #绘制车道线
    for point in line_point:
        out_img = cv2.circle(img, (int(point[0]),int(point[1])), 2, [0,0,255], -1)  
    return out_img

#将图片转换到HSV色彩空间，提取白色
def get_whiteline(img):
    #将BGR图像转换为HSV  
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)  
    lower_white = np.array([0, 0, 220])      # 白色HSV范围的下限（这里假设较亮的颜色为白色）  
    upper_white = np.array([180, 30, 255])   # 白色HSV范围的上限  
    mask_white = cv2.inRange(hsv, lower_white, upper_white)  
    return mask_white

def get_open(img):
    ret, thresholded = cv2.threshold(img, 100, 255, cv2.THRESH_BINARY)  
    # 定义结构元素，用于腐蚀和膨胀操作  
    kernel = np.ones((5, 5), np.uint8)  # 定义一个5x5的结构元素  
    # 开运算
    opened = cv2.morphologyEx(thresholded, cv2.MORPH_CLOSE, kernel) 
    return opened

def process_image(img):
    #将BGR图像转换为HSV  
    img_hsv = get_whiteline(img)
    #获取灰度图
    # img_gray = cv2.cvtColor(img_hsv, cv2.COLOR_HSV2GRAY)
    #腐蚀膨胀去除噪点
    img_open =get_open(img_hsv)
    # 车道线检测
	#rigin_pipline_img = pipeline(img)
    # 拟合车道线
    left_fit,out_img = cal_line_param(img_open,img)
    # 绘制车道线
    result = fill_lane_poly(img,left_fit)
    return result

